#include "mainwindow.h"

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
{
    qDebug() << "desktop width:" << QApplication::desktop()->width();
    qDebug() << "desktop height:" << QApplication::desktop()->height();
    qDebug() << "desktop physicalDpiX:" << QApplication::desktop()->physicalDpiX();
    qDebug() << "desktop physicalDpiY:" << QApplication::desktop()->physicalDpiY();
    qDebug() << "screen0 physicalSize:" << QGuiApplication::screens().at(0)->physicalSize();
    qDebug() << "screen0 physicalDotsPerInch:" << QGuiApplication::screens().at(0)->physicalDotsPerInch();
    qDebug() << "screen0 logicalDotsPerInch:" << QGuiApplication::screens().at(0)->logicalDotsPerInch();
    this->resize(800,640);
    this->setWindowTitle("Mavlink-Sender");
    this->setWindowIcon(QIcon("../resource/main_window_icon.png"));// TODO:支持配置
    // 窗口背景色
    QColor color = QColor(Qt::white);
    QPalette p = this->palette();
    p.setColor(QPalette::Window,color);
    this->setPalette(p);

    mainWindowCentralWidget = new QWidget();
    vBoxLayoutCentralWidget = new QVBoxLayout();
    vBoxLayoutCentralWidget->setMargin(0);
    vBoxLayoutCentralWidget->setSpacing(0);
    mainWindowCentralWidget->setLayout(vBoxLayoutCentralWidget);

    udpSocket = new QUdpSocket;
    udpPort = 14550;
    sendSizeTotal = 0;
    heartbeatCount = 0;
    packagesCount = 0;
    timer = new QTimer;
    timer->setInterval(timerLoopTime);
    connect(timer, &QTimer::timeout, this, &MainWindow::sendLoop);


    radioButtonSend = new QRadioButton("SEND");
    vBoxLayoutCentralWidget->addWidget(radioButtonSend);
    connect(radioButtonSend, &QRadioButton::clicked, this, &MainWindow::changeUdpSendState);
    radioButtonSend->setChecked(true);
    changeUdpSendState();

    this->setAttribute(Qt::WA_DeleteOnClose);
    this->setCentralWidget(mainWindowCentralWidget);

    //状态栏
    windowStatusBar = statusBar();

    labelStatusUDPSpeed = new QLabel("UDP:0B/s");

    pixelSize = labelStatusUDPSpeed->fontInfo().pixelSize() / 2;
    labelStatusUDPSpeed->adjustSize();
    qDebug() << labelStatusUDPSpeed->text().size();
    qDebug() << labelStatusUDPSpeed->size();
    labelStatusUDPSpeed->setMinimumWidth(pixelSize * (labelStatusUDPSpeed->text().size() + 4));
    windowStatusBar->addWidget(labelStatusUDPSpeed);

    labelStatusPackages = new QLabel("Packages:0");
    labelStatusPackages->setMinimumWidth(pixelSize * (labelStatusPackages->text().size() + 4));
    windowStatusBar->addWidget(labelStatusPackages);

    labelStatusHeartbeat = new QLabel("Heartbeat:0");
    labelStatusHeartbeat->setMinimumWidth(pixelSize * (labelStatusHeartbeat->text().size() + 4));
    windowStatusBar->addWidget(labelStatusHeartbeat);

    labelStatusHostPlatform = new QLabel();
#ifdef __HOST_PLATFORM_WINDOWS_
    labelStatusHostPlatform->setText(QString("Host windows"));
//    labelStatusHostPlatform->adjustSize();
#elif defined __HOST_PLATFORM_LINUX_
    labelStatusHostPlatform->setText(QString("Host linux"));
//    labelStatusHostPlatform->adjustSize();
#else
    labelStatusHostPlatform->setText(QString("Host unknown"));
//    labelStatusHostPlatform->adjustSize();
#endif
    windowStatusBar->addWidget(labelStatusHostPlatform);
}

MainWindow::~MainWindow()
{
    qDebug() << "File:" <<  __FILE__ << " Line:"<<  __LINE__ << " Func:" <<  __FUNCTION__;

    this->close();
}

void MainWindow::resizeEvent(QResizeEvent *event)
{
    qDebug() << "size0:" << labelStatusUDPSpeed->size();
    qDebug() << "size1:" << labelStatusUDPSpeed->sizeHint();
    qDebug() << labelStatusUDPSpeed->fontInfo().pixelSize();
}

void MainWindow::changeUdpSendState()
{
    if(radioButtonSend->isChecked())
    {
        qDebug() << "start send now";
        timer->start();
    } else {
        qDebug() << "stop send now";
        timer->stop();
    }
}

void MainWindow::sendLoop()
{
    static quint64 sendSizeTotalLast = 0;
    bootTime += timerLoopTime;
    if(udpSocket == nullptr)
        return;

    if(bootTime % 20 == 0)
    {
        // 50Hz
        mavlinkGenerate(MAVLINK_MSG_ID_ATTITUDE);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_ATTITUDE_QUATERNION);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_ATTITUDE_TARGET);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_GLOBAL_POSITION_INT);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_LOCAL_POSITION_NED);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;
    }
    if(bootTime % 250 == 0)
    {
        // 4Hz
        mavlinkGenerate(MAVLINK_MSG_ID_VFR_HUD);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;
    }
    if(bootTime % 500 == 0)
    {
        // 2Hz
        mavlinkGenerate(MAVLINK_MSG_ID_ESC_STATUS);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;
    }
    if(bootTime % 1000 == 0)
    {
        // 1Hz
//        qDebug() << "1Hz, boottime: " << bootTime;
        mavlinkGenerate(MAVLINK_MSG_ID_ALTITUDE);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_GPS_RAW_INT);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_LINK_NODE_STATUS);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_SYS_STATUS);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_HEARTBEAT);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        heartbeatCount++;
        updateLabelHeartbeat(heartbeatCount);

        updateLabelPackages(packagesCount);

        updateLabelUdpSpeed(sendSizeTotal - sendSizeTotalLast);
        sendSizeTotalLast = sendSizeTotal;
    }
    if(bootTime % 2000 == 0)
    {
        // 0.5Hz
        mavlinkGenerate(MAVLINK_MSG_ID_BATTERY_STATUS);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_ESTIMATOR_STATUS);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_HOME_POSITION);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_UTM_GLOBAL_POSITION);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;
    }
    if(bootTime % 10000 == 0)
    {
        // 0.1Hz
        mavlinkGenerate(MAVLINK_MSG_ID_PING);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;

        mavlinkGenerate(MAVLINK_MSG_ID_VIBRATION);
        udpSocket->writeDatagram((char*)sendBuffer, sendSize, QHostAddress::Broadcast, udpPort);
        sendSizeTotal += sendSize;
    }

}

void MainWindow::updateLabelUdpSpeed(quint64 speedBytes)
{
    QString string;
    string.clear();
    string += "UDP:";
    string += QString::number(speedBytes);
    string += "B/s";
    labelStatusUDPSpeed->setText(string);
    labelStatusUDPSpeed->setMinimumWidth(pixelSize * (labelStatusUDPSpeed->text().size() + 4));
}

void MainWindow::updateLabelHeartbeat(quint32 count)
{
    QString string;
    string.clear();
    string += "Heartbeat:";
    string += QString::number(count);
    labelStatusHeartbeat->setText(string);
    labelStatusHeartbeat->setMinimumWidth(pixelSize * (labelStatusHeartbeat->text().size() + 4));
}

void MainWindow::updateLabelPackages(quint32 count)
{
    QString string;
    string.clear();
    string += "Packages:";
    string += QString::number(count);
    labelStatusPackages->setText(string);
    labelStatusPackages->setMinimumWidth(pixelSize * (labelStatusPackages->text().size() + 4));
}

void MainWindow::mavlinkGenerate(quint32 msgid)
{
    memset(&message, 0, sizeof(message));
//    message.sysid = 1;
//    message.compid = 1;
//    message.msgid = msgid;

    switch (msgid) {
    case MAVLINK_MSG_ID_ALTITUDE:
        mavlink_altitude_t altitude;// 141
        altitude.time_usec = 0;
        altitude.altitude_monotonic = 487.35;
        altitude.altitude_amsl = 488.105;
        altitude.altitude_local = 0.06822;
        altitude.altitude_relative = 0.052;
        altitude.altitude_terrain = -0.032;
        altitude.bottom_clearance = 0.1;
        mavlink_msg_altitude_encode(1, 1, &message, &altitude);
        break;

    case MAVLINK_MSG_ID_ATTITUDE:
        mavlink_attitude_t attitude;// 30
        attitude.pitch = 0.1;
        attitude.roll = 0.2;
        attitude.yaw = 0.3;
        attitude.time_boot_ms = bootTime;
        mavlink_msg_attitude_encode(1, 1, &message, &attitude);
        break;

    case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
        mavlink_attitude_quaternion_t attitude_quaternion;// 31
        attitude_quaternion.time_boot_ms = bootTime;
        attitude_quaternion.q1 = 0.708;
        attitude_quaternion.q2 = 0.00001;
        attitude_quaternion.q3 = 0.00001;
        attitude_quaternion.q4 = 0.705;
        attitude_quaternion.repr_offset_q[0] = 0;
        attitude_quaternion.repr_offset_q[1] = 0;
        attitude_quaternion.repr_offset_q[2] = 0;
        attitude_quaternion.repr_offset_q[3] = 0;
        mavlink_msg_attitude_quaternion_encode(1, 1, &message, &attitude_quaternion);
        break;

    case MAVLINK_MSG_ID_ATTITUDE_TARGET:
        mavlink_attitude_target_t attitude_target;// 83
        attitude_target.type_mask = 0;
        attitude_target.thrust = 0.001;
        mavlink_msg_attitude_target_encode(1, 1, &message, &attitude_target);
        break;

    case MAVLINK_MSG_ID_BATTERY_STATUS:
        mavlink_battery_status_t battery_status;// 147
        battery_status.id = 0;
        battery_status.battery_function = 1;
        battery_status.type = 1;
        mavlink_msg_battery_status_encode(1, 1, &message, &battery_status);
        break;

    case MAVLINK_MSG_ID_ESC_STATUS:
        mavlink_esc_status_t esc_status;// 291
        esc_status.index = 4;
        esc_status.time_usec = bootTime * 1000;
        mavlink_msg_esc_status_encode(1, 1, &message, &esc_status);
        break;

    case MAVLINK_MSG_ID_ESTIMATOR_STATUS:
        mavlink_estimator_status_t estimator_status;// 230
        estimator_status.time_usec = bootTime * 1000;
        estimator_status.flags = 895;
        mavlink_msg_estimator_status_encode(1, 1, &message, &estimator_status);
        break;

    case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
        mavlink_extended_sys_state_t extended_sys_state;// 245
        extended_sys_state.vtol_state = 0;
        extended_sys_state.landed_state = 1;
        mavlink_msg_extended_sys_state_encode(1, 1, &message, &extended_sys_state);
        break;

    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
        mavlink_global_position_int_t global_position_int;// 33
        global_position_int.time_boot_ms = bootTime;
        global_position_int.vx = 1;
        global_position_int.lat = 457366688;
        global_position_int.lon = 1266281588;
        mavlink_msg_global_position_int_encode(1, 1, &message, &global_position_int);
        break;

    case MAVLINK_MSG_ID_GPS_RAW_INT:
        mavlink_gps_raw_int_t gps_raw_int;// 24
        gps_raw_int.time_usec = bootTime * 1000;
        gps_raw_int.lat = 457366688;
        gps_raw_int.lon = 1266281588;
        mavlink_msg_gps_raw_int_encode(1, 1, &message, &gps_raw_int);
        break;

    case MAVLINK_MSG_ID_HOME_POSITION:
        mavlink_home_position_t home_position;// 242
        home_position.time_usec = bootTime * 1000;
        home_position.latitude = 457366688;
        home_position.longitude = 1266281588;
        mavlink_msg_home_position_encode(1, 1, &message, &home_position);
        break;

    case MAVLINK_MSG_ID_LINK_NODE_STATUS:
        mavlink_link_node_status_t link_node_status;// 8
        link_node_status.timestamp = bootTime * 1000;
        mavlink_msg_link_node_status_encode(1, 1, &message, &link_node_status);
        break;

    case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
        mavlink_local_position_ned_t local_position_ned;// 32
        local_position_ned.time_boot_ms = bootTime;
        local_position_ned.vx = 0.002;
        mavlink_msg_local_position_ned_encode(1, 1, &message, &local_position_ned);
        break;

    case MAVLINK_MSG_ID_PING:
        mavlink_ping_t ping;// 4
        ping.time_usec = bootTime * 1000;
        ping.seq = 222;
        mavlink_msg_ping_encode(1, 1, &message, &ping);
        break;

    case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
        mavlink_position_target_local_ned_t position_target_local_ned;// 85
        position_target_local_ned.time_boot_ms = bootTime;
        position_target_local_ned.type_mask = 63;
        position_target_local_ned.vy = 0.1;
        mavlink_msg_position_target_local_ned_encode(1, 1, &message, &position_target_local_ned);
        break;

    case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
        mavlink_servo_output_raw_t servo_output_raw;// 36
        servo_output_raw.time_usec = bootTime * 1000;
        servo_output_raw.servo1_raw = 900;
        servo_output_raw.servo2_raw = 901;
        mavlink_msg_servo_output_raw_encode(1, 1, &message, &servo_output_raw);
        break;

    case MAVLINK_MSG_ID_SYS_STATUS:
        mavlink_sys_status_t sys_status;// 1
        sys_status.load = 1450;
        mavlink_msg_sys_status_encode(1, 1, &message, &sys_status);
        break;

    case MAVLINK_MSG_ID_UTM_GLOBAL_POSITION:
        mavlink_utm_global_position_t utm_global_position;// 340
        utm_global_position.relative_alt = 100;
        utm_global_position.h_acc = 1001;
        utm_global_position.flight_state = 1;
        mavlink_msg_utm_global_position_encode(1, 1, &message, &utm_global_position);
        break;

    case MAVLINK_MSG_ID_VFR_HUD:
        mavlink_vfr_hud_t vfr_hud;// 74
        vfr_hud.groundspeed = 0.003;
        vfr_hud.heading = 90;
        mavlink_msg_vfr_hud_encode(1, 1, &message, &vfr_hud);
        break;

    case MAVLINK_MSG_ID_VIBRATION:
        mavlink_vibration_t vibration;// 241
        vibration.time_usec = bootTime * 1000;
        vibration.vibration_y = 0.002;
        mavlink_msg_vibration_encode(1, 1, &message, &vibration);
        break;

    default:
        // send heart beat

    case MAVLINK_MSG_ID_HEARTBEAT:
        mavlink_heartbeat_t heartbeat;// 0
        heartbeat.type = 2;
        heartbeat.autopilot = 12;
        heartbeat.base_mode = 29;
        heartbeat.custom_mode = 50593792;
        heartbeat.system_status = 3;
        heartbeat.mavlink_version = 3;
        mavlink_msg_heartbeat_encode(1, 1, &message, &heartbeat);
        break;
    }
    sendSize = mavlink_msg_to_send_buffer(sendBuffer, &message);
    packagesCount++;
}
